
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       xrce_dds_header.c
  * @author     baiyang
  * @date       2022-4-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "xrce_dds_header.h"
#include "xrce_dds_topic_common.h"

#include <xrce_dss/xrce_dds.h>
#include <common/time/gp_time.h>

#include <mavproxy/mavproxy.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
uint16_t header_object_id;

static uxrObjectId datareader_id;
static uxrObjectId datawriter_id;

static XrceDDSHeader topic;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void xrce_dds_header_on_topic(uxrObjectId object_id, struct ucdrBuffer* ub)
{
    XrceDDSHeader topic;
    Header_deserialize_topic(ub, &topic);

    mavproxy_send_statustext(MAV_SEVERITY_INFO, "xrce_dds: Receive message, sec: %d, nanosec: %d", topic.stamp.sec, topic.stamp.nanosec);
}

void xrce_dds_header_init()
{
    XRCE_DDS_HEADER_OBJECT_ID = ASSIGN_TOPIC_ID;
    xrce_dds_topic_create_entities(XRCE_DDS_HEADER_OBJECT_ID, "rt/microROS/ping", "std_msgs::msg::dds_::Header_", NULL, &datawriter_id, NULL, NULL);
}

bool xrce_dds_header_run(uint32_t time_now_ms)
{
    bool connected = true;


    DEFINE_TIMETAG(topic_send, 1000);

    if (time_check_tag2(TIMETAG(topic_send), time_now_ms)) {
        uint64_t time_now_us = time_micros64();
        topic.stamp.sec = time_now_us * 0.000001;
        topic.stamp.nanosec = (time_now_us % 1000000) * 1e3;

        ucdrBuffer ub;
        uint32_t topic_size = Header_size_of_topic(&topic, 0);
        uxr_prepare_output_stream(&xrce_dds.xrce_dds_session, xrce_dds.besteffort_out, datawriter_id, &ub, topic_size);
        Header_serialize_topic(&ub, &topic);

        // 可靠传输（reliable），发送数据后，代理端会有回复。
        // 尽力而为的传输（best_effort），发送数据后，代理端没有回复。
        // uxr_run_session_time 如果是可靠传输在timeout_ms时间内没有收到回复，会返回false
        // 如果是尽力而为的传输始终返回true
        connected = uxr_run_session_time(&xrce_dds.xrce_dds_session, 0);

    }

    if (!connected) {
        return false;
    }

    return true;
}

/*------------------------------------test------------------------------------*/


